#include <moying_costmap/moying_costmap.h>
#include <dispatch_system_ros_viz/moyingmap_viz.h>
#include <dispatch_system_ros_viz/vehicle_viz.h>
#include <dispatch_system/time_based_astar.h>
#include <dispatch_system/diff_drive_vehicle.h>
#include <dispatch_system/mecanum_vehicle.h>

#include <ros/ros.h>
#include <ros/package.h>
using namespace moying::navigation;
using namespace moying::visualization;

MoyingMapPtr moyingmap;
MoyingMapVizPtr viz;
std::vector<VehiclePtr> vehicles;
std::vector<PlanningResult> results;

int main(int argc, char **argv)
{   
    ros::init(argc, argv, "ros_map_test");
    ros::NodeHandle nh("~");

    std::string pack_path = ros::package::getPath("dispatch_system_ros_viz");

    CostMapPtr costmap(new CostMap());
    std::vector<double> origin = {-25.0, -25.0, 0.000000};
    if(!costmap->loadStaticMapFromFile(pack_path + "/resources/test_1000_1000.pgm", 0.05, 0, 0.65, 0.196, origin))
    {
        ERROR("Load costmap failed");
        return 0;
    }
    costmap->inflateStaticMap(0.5);
    costmap->setupPlanningCostMap(0.05);

    moyingmap = std::make_shared<MoyingMap>();
    moyingmap->init(costmap);
    moyingmap->initializeTimedMap(600, 1);

    viz = std::make_shared<MoyingMapViz>(moyingmap);

    TimeBasedAStarPtr astar = std::make_shared<TimeBasedAStar>();
    astar->init(moyingmap);
    vehicles.push_back(std::make_shared<MecanumVehicle>("1", "V1"));
    // vehicles.push_back(std::make_shared<MecanumVehicle>("1", "V2"));
    // vehicles.push_back(std::make_shared<Vehicle>("1", "V3"));
    // vehicles.push_back(std::make_shared<Vehicle>("1", "V4"));
    for(int i = 0 ; i < vehicles.size(); i++)
        vehicles[i]->setMovePatterns();
    
    std::vector<VehicleState> starts, goals;
    VehicleState start1, goal1;
    start1.x = -18;
    start1.y = -18;
    start1.th = 0;
    goal1.x = 10;
    goal1.y = -18;
    goal1.th = 0;
    starts.push_back(start1);
    goals.push_back(goal1);

    VehicleState start2, goal2;
    start2.x = -1.2;
    start2.y = -1.5;
    start2.th = 0;
    goal2.x = 3.6;
    goal2.y = -1;
    goal2.th = 0;
    starts.push_back(start2);
    goals.push_back(goal2);

    std::vector<VehicleVizPtr> vehicle_viz;
    for(int i = 0 ; i < vehicles.size(); i++)
        vehicle_viz.push_back(std::make_shared<VehicleViz>(vehicles[i], moyingmap));

    results.resize(vehicles.size());
    HIGHLIGHT("Start Testing");
    for(int i = 0 ; i < vehicles.size(); i++)
    {
        ros::Time start_time = ros::Time::now();
        starts[i].t = moyingmap->getNextPlanningTime();
        astar->searchPath(vehicles[i], starts[i], goals[i], results[i]);
        ROS_INFO_STREAM(vehicles[i]->getName()<<" Planning time "<<ros::Duration(ros::Time::now() - start_time).toSec());
        vehicle_viz[i]->publishTrajectory(results[i]);
        vehicle_viz[i]->publishCellTrajectory(results[i].timed_footprints);
    }
    // viz->publishTimedMap();

    ros::spin();
    return 0;
    
}
